﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathLib;

namespace TrackingSim.Filtering
{
    class State6PosVel
    {
        Vector state;

        public State6PosVel()
        {
            state = new Vector(6);
        }

        public State6PosVel(double pos_x, double pos_y, double pos_z, double vel_x, double vel_y, double vel_z)
        {
            state = new Vector(6);
            X = pos_x;
            Y = pos_y;
            Z = pos_z;
            Xdot = vel_x;
            Ydot = vel_y;
            Zdot = vel_z;
        }

        public State6PosVel clone()
        {
            State6PosVel copy = new State6PosVel(X, Y, Z, Xdot, Ydot, Zdot);
            return copy;
        }

        public Vector State
        {
            get { return state; }
            set { state = value; }
        }

        public double X
        {
            get { return state.v[0]; }
            set { state.v[0] = value; }
        }

        public double Y
        {
            get { return state.v[1]; }
            set { state.v[1] = value; }
        }

        public double Z
        {
            get { return state.v[2]; }
            set { state.v[2] = value; }
        }

        public double Xdot
        {
            get { return state.v[3]; }
            set { state.v[3] = value; }
        }

        public double Ydot
        {
            get { return state.v[4]; }
            set { state.v[4] = value; }
        }

        public double Zdot
        {
            get { return state.v[5]; }
            set { state.v[5] = value; }
        }

        public Vector Pos
        {
            get
            {
                Vector pos = new Vector(3);
                pos.v[0] = X;
                pos.v[1] = Y;
                pos.v[2] = Z;
                return pos;
            }

            set
            {
                X = value.v[0];
                Y = value.v[1];
                Z = value.v[2];
            }
        }

        public Vector Vel
        {
            get
            {
                Vector vel = new Vector(3);
                vel.v[4] = Xdot;
                vel.v[5] = Ydot;
                vel.v[6] = Zdot;
                return vel;
            }

            set
            {
                if (value.N == 6)
                {
                    Xdot = value.v[4];
                    Ydot = value.v[5];
                    Zdot = value.v[6];
                }
                else
                {
                    Xdot = value.v[0];
                    Ydot = value.v[1];
                    Zdot = value.v[2];
                }
            }
        }

        public void set(double pos_x, double pos_y, double pos_z, double vel_x, double vel_y, double vel_z)
        {
            X = pos_x;
            Y = pos_y;
            Z = pos_z;
            Xdot = vel_x;
            Ydot = vel_y;
            Zdot = vel_z;
        }

        public double pos_mag()
        {
            return Math.Sqrt(X * X + Y * Y + Z * Z);
        }

        public double vel_mag()
        {
            return Math.Sqrt(Xdot * Xdot + Ydot * Ydot + Zdot * Zdot);
        }
    }
}
